/*
 * CameraStateValidityChecker.cpp
 *
 *  Created on: Mar 22, 2013
 *      Author: ohnozzy
 */

#include "CameraStateValidityChecker.h"
#include "flyingcamera/conversion/Conversion.h"
#include <iostream>
using namespace flyingcamera::base;

bool CameraStateValidityChecker::isValid(const ompl::base::State *state) const
{

	static int count=0;
	count++;
	const CameraSpace::StateType *cstate = state->as<CameraSpace::StateType>();
	const ompl::base::SO3StateSpace::StateType& rotation=cstate->rotation();
	/*if(rotation.w<0.706)
        		  return false;*/
    double t=cstate->getT();
    //std::cout<<"t:"<<t<<std::endl;


    double x, y, Yaw;
    fcl::Quaternion3f fcl_r;
    fcl_r.getW () = rotation.w;
    fcl_r.getX () = rotation.x;
    fcl_r.getY () = rotation.y;
    fcl_r.getZ () = rotation.z;

    fcl::Transform3f rtf(fcl_r);
    path->getLocation(t, x, y, Yaw);
    flyingcamera::conversion::moveUpandTranslate(rtf, robot.lz, x, y);
    fcl::Matrix3f rm;
    fcl_r.toRotation(rm);
    //Prevent the camera going under the ground
    if(0.5*rm(2,2)*robot.lz+rtf.getTranslation()[2]<0)
    	return false;
    for(unsigned i=0; i<obstacle.size();i++){
    	fcl::CollisionRequest collisionRequest;
    	fcl::CollisionResult collisionResult;
    	const fcl::CollisionGeometry* o=obstacle[i];
    	const fcl::Transform3f& otf=obstacle_tf[i];

    	std::size_t collision_szie=fcl::collide(&robot, rtf, o, otf,collisionRequest,collisionResult);

    	if(collision_szie>0){
    		return false;
    	}
    }


    return true;
}
void CameraStateValidityChecker::addObstacle(fcl::CollisionGeometry* const o, const fcl::Transform3f& tf){
	obstacle.push_back(o);
	obstacle_tf.push_back(tf);
}


